#include <AccelStepper.h>
#include <WiFi.h>
#include <PubSubClient.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1
#define DIR_PIN  D0
#define EN_PIN   D3

#define M0_PIN   D2
#define M1_PIN   D4
#define M2_PIN   D5

#define HALL_PIN D8

// ================= WIFI =================
const char* ssid = "Ali";
const char* password = "00000000";

// ================= MQTT =================
const char* mqtt_server = "mqtt.fabcloud.org";
const int mqtt_port = 1883;
const char* mqtt_user = "fabacademy";
const char* mqtt_pass = "fabacademy";

const char* sub_topic = "fabacademy/ashtami/tool";

WiFiClient espClient;
PubSubClient client(espClient);

// ================== HALL THRESHOLDS ==================
#define ON_THRESHOLD  3800
#define OFF_THRESHOLD 3400

// ================== TURNTABLE CONSTANTS ==================
#define MOTOR_STEPS   200
#define MICROSTEPS    16
#define GEAR_RATIO    5
#define STEPS_PER_REV (MOTOR_STEPS * MICROSTEPS * GEAR_RATIO)  // 16000

// ================== SPEED SETTINGS ==================
#define SEARCH_SPEED    800
#define BACKOFF_SPEED   400
#define APPROACH_SPEED  200

#define MOVE_MAX_SPEED  1200
#define MOVE_ACCEL      600

// ================== LOOKUP TABLE ==================
// Absolute positions (pre-calibrated, drift-free)
const long LUT[7][5] = {
  {0,      0,      0,      0,      0     },

  {0,      0,   4000,   8000,  12000    }, // C1
  {0,   2665,   6665,  10665,  14665    }, // C2
  {0,   5332,   9332,  13332,   1332    }, // C3
  {0,   7999,  11999,  15999,   3999    }, // C4
  {0,  10666,  14666,   2666,   6666    }, // C5
  {0,  13333,   1333,   5333,   9333    }, // C6
};

const char* COMP_LABEL[7] = {"", "C1","C2","C3","C4","C5","C6"};
const char* HOME_LABEL[5] = {"", "H1","H2","H3","H4"};

// ================== STEPPER ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL FILTER ==================
float filtered  = 0.0f;
bool  hallState = false;

// ================== STATE MACHINES ==================
enum HomingState { SEARCH, BACKOFF, APPROACH, HOMING_DONE };
enum SystemState { HOMING, POSITIONING, IDLE };

HomingState homingState = SEARCH;
SystemState sysState    = HOMING;

// ================== POSITION TRACKING ==================
long turntableStepPos = 0;  // 0 → 15999 absolute
int  lastComp = 0;
int  lastHome = 0;

// ================== HALL SENSOR ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);

  // Low-pass filter
  filtered = 0.8f * filtered + 0.2f * raw;

  // Hysteresis
  if (!hallState && filtered > ON_THRESHOLD) hallState = true;
  if ( hallState && filtered < OFF_THRESHOLD) hallState = false;

  return hallState;
}

// ================== HOMING ==================
void runHoming() {

  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (homingState) {

    case SEARCH:
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {
        Serial.println("[HOMING] Magnet detected → BACKOFF");
        homingState = BACKOFF;
      }
      break;

    case BACKOFF:
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();

      if (!currentHall && lastHall) {
        Serial.println("[HOMING] Magnet cleared → APPROACH");
        homingState = APPROACH;
      }
      break;

    case APPROACH:
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();

      if (currentHall && !lastHall) {

        stepper.setSpeed(0);

        // Define absolute zero
        // Guarantees: C1 = H1
        stepper.setCurrentPosition(0);
        turntableStepPos = 0;

        lastComp = 1;
        lastHome = 1;

        // Switch to motion mode
        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        stepper.setAcceleration(MOVE_ACCEL);

        homingState = HOMING_DONE;
        sysState    = IDLE;

        Serial.println("HOMING COMPLETE → C1 aligned with H1");
      }
      break;

    case HOMING_DONE:
      break;
  }

  lastHall = currentHall;
}

// ================== MOVE (SHORTEST PATH) ==================
void moveCompartmentToHome(int c, int h) {

  if (c < 1 || c > 6) { Serial.println("[ERR] C must be 1–6"); return; }
  if (h < 1 || h > 4) { Serial.println("[ERR] H must be 1–4"); return; }

  long target = LUT[c][h];

  // -------- SHORTEST PATH CALCULATION --------

  // Raw difference
  long delta = target - turntableStepPos;

  // Normalize to shortest path range [-8000, +8000]
  if (delta >  STEPS_PER_REV / 2)  delta -= STEPS_PER_REV;
  if (delta < -STEPS_PER_REV / 2)  delta += STEPS_PER_REV;

  if (delta == 0) {
    Serial.println("[INFO] Already at target");
    return;
  }

  // Command relative move
  stepper.moveTo(stepper.currentPosition() + delta);
  sysState = POSITIONING;

  // Update logical state
  turntableStepPos = target;
  lastComp = c;
  lastHome = h;

  // Debug info
  Serial.print("Moving ");
  Serial.print(COMP_LABEL[c]);
  Serial.print(" → ");
  Serial.println(HOME_LABEL[h]);

  Serial.print("Steps: ");
  Serial.println(delta);

  Serial.print("Direction: ");
  Serial.println(delta > 0 ? "CW" : "CCW");
}

// ================== SERIAL ==================
void parseCommand(String cmd) {

  cmd.trim();
  cmd.toUpperCase();

  // Format: CnHm
  if (cmd.length() == 4 && cmd[0]=='C' && cmd[2]=='H') {
    int c = cmd[1] - '0';
    int h = cmd[3] - '0';
    moveCompartmentToHome(c, h);
    return;
  }

  if (cmd == "HOME") {
    homingState = SEARCH;
    sysState    = HOMING;
    Serial.println("[HOMING] Restarting...");
    return;
  }

  if (cmd == "POS") {
    Serial.print("Steps: ");
    Serial.println(turntableStepPos);
    return;
  }

  Serial.println("[ERR] Unknown command");
}

// ================= MQTT CALLBACK =================
void callback(char* topic, byte* payload, unsigned int length) {

  Serial.println("\n🔥 MQTT MESSAGE RECEIVED");

  String msg = "";

  for (int i = 0; i < length; i++) {
    msg += (char)payload[i];
  }

 Serial.print("📩 MQTT RAW: ");
  Serial.println(msg);

  // 🔥 FIX HERE
  msg.replace(":", "");

  Serial.print("📩 Cleaned: ");
  Serial.println(msg);

  // 👇 Your original system
  parseCommand(msg);
}

// ================= WIFI =================
void setup_wifi() {
  Serial.print("Connecting WiFi");

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("\n✅ WiFi Connected");
}

// ================= MQTT =================
void reconnect() {
  while (!client.connected()) {

    Serial.println("🔌 Connecting MQTT...");

    if (client.connect("ESP32S3", mqtt_user, mqtt_pass)) {

      Serial.println("✅ MQTT Connected");

      client.subscribe(sub_topic);

      Serial.print("📡 Subscribed: ");
      Serial.println(sub_topic);

    } else {
      Serial.print("❌ Failed rc=");
      Serial.println(client.state());
      delay(2000);
    }
  }
}

// ================== SETUP ==================
void setup() {

  Serial.begin(115200);

  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  // DRV8825 → 1/16 microstepping
  pinMode(M0_PIN, OUTPUT); digitalWrite(M0_PIN, LOW);
  pinMode(M1_PIN, OUTPUT); digitalWrite(M1_PIN, LOW);
  pinMode(M2_PIN, OUTPUT); digitalWrite(M2_PIN, HIGH);

  filtered = analogRead(HALL_PIN);

  stepper.setMaxSpeed(SEARCH_SPEED);

  Serial.println("Starting homing...");

// wifi and mqtt
  setup_wifi();

    client.setServer(mqtt_server, mqtt_port);
    client.setCallback(callback);

    Serial.println("MQTT Ready");
}

// ================== LOOP ==================



void loop() {

  if (!client.connected()) reconnect();
client.loop(); // alexa code

  switch (sysState) {
  
    case HOMING:
      runHoming();
      break;

    case POSITIONING:
      if (stepper.distanceToGo() != 0) {
        stepper.run();
      } else {
        Serial.println("Move complete");
        sysState = IDLE;
      }
      break;

    case IDLE:
      if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        parseCommand(cmd);
      }
      break;
  }
}